/*
 * Copyright (c) 2020 - ~, HIT_HERO Team
 *
 * 2508MOTOR_CAN MODULE SOUCE FILE
 * Used in RT-Thread Operate System
 *
 * Change Logs:
 * Date           Author       Notes            	Mail
 * 2020-08-08     WangXi       first version    	WangXi_chn@foxmail.com
 * 
 * Note:
 * DJI 3508 motor and C620 motor controler driver and control framework
 * All paramemters shall be subject to the motor rotor control
 * And the parameters of the shaft shall be converted by yourself if needed
 * The positive direction is clockwise facing the rotor
 */
 
#include "Module_DjiC610620Group.h"

/* User Code Begin*/


/* User Code End */

/* Static Method */

static void 	Module_DjiC610620GroupInit(MODULE_DjiC610620GROUP *module);
static void 	Module_DjiC610620GroupSend(MODULE_DjiC610620GROUP *module);
static void 	Module_DjiC610620GroupFeed(MODULE_DjiC610620GROUP *module);
static rt_err_t Module_DjiC610620GroupHandle(rt_device_t dev, rt_size_t size);

static rt_err_t Module_DjiC610620_Config(MODULE_DjiC610620 *Dev_DjiC610620);
static void 	Module_DjiC610620Init(MODULE_DjiC610620 *module);

static struct rt_semaphore can3508Motor_sem;


/* Global Method */
rt_err_t Module_DjiC610620Group_Config(MODULE_DjiC610620GROUP *Dev_DjiC610620Group)
{
	if( Dev_DjiC610620Group->Method_Init		==NULL &&
		Dev_DjiC610620Group->Method_Feed		==NULL &&
		Dev_DjiC610620Group->Method_Send		==NULL &&
		Dev_DjiC610620Group->Method_Handle	==NULL
		
    ){  
        /* Link the Method */
        Dev_DjiC610620Group->Method_Init 	= Module_DjiC610620GroupInit;
		Dev_DjiC610620Group->Method_Feed		= Module_DjiC610620GroupFeed;
		Dev_DjiC610620Group->Method_Send		= Module_DjiC610620GroupSend;
		Dev_DjiC610620Group->Method_Handle	= Module_DjiC610620GroupHandle;
			
    }
    else{
        rt_kprintf("Warning: Module Motor 3508 is Configed twice\n");
        return RT_ERROR;
    }

    /* Device Init */
    Dev_DjiC610620Group->Method_Init(Dev_DjiC610620Group);
	
	/* Motor Config */
	for(int i=0;i<8;i++)
	{
		if(Dev_DjiC610620Group->Value_module_DjiC610620[i].Enable != 0)
		{
			Dev_DjiC610620Group->Value_motorUsedMask |= 0x01<<i; 
			Module_DjiC610620_Config(&(Dev_DjiC610620Group->Value_module_DjiC610620[i]));
		}
	}

    return RT_EOK;
}

static void Module_DjiC610620GroupInit(MODULE_DjiC610620GROUP *module)
{
	rt_err_t res;
	
	/* link can devices */
	module->Value_can_dev = rt_device_find(module->Property_CanDevName);
	if (!module->Value_can_dev)
        rt_kprintf("find %s failed!\n", module->Property_CanDevName);
	
	/* init semaphone */
	rt_sem_init(&can3508Motor_sem, "can3508Motor_sem", 0, RT_IPC_FLAG_FIFO);
	
	module->Value_can_msgLow.id = 0x200;
	module->Value_can_msgLow.ide = RT_CAN_STDID;
	module->Value_can_msgLow.rtr = RT_CAN_DTR;
	module->Value_can_msgLow.len = 8;
	module->Value_can_msgHigh.id = 0x1FF;
	module->Value_can_msgHigh.ide = RT_CAN_STDID;
	module->Value_can_msgHigh.rtr = RT_CAN_DTR;
	module->Value_can_msgHigh.len = 8;
	module->Value_can_mrg.hdr = -1;
	
	res = rt_device_open(module->Value_can_dev, RT_DEVICE_FLAG_INT_TX | RT_DEVICE_FLAG_INT_RX);
	RT_ASSERT(res == RT_EOK);	
	
	/* set up can baudrate */
	rt_device_control(module->Value_can_dev, RT_CAN_CMD_SET_BAUD, (void *)CAN1MBaud);
	
	/* link call back function */
	rt_device_set_rx_indicate(module->Value_can_dev, module->Method_Handle);
	

}

rt_err_t Module_DjiC610620_Config(MODULE_DjiC610620 *Dev_DjiC610620)
{
	if( Dev_DjiC610620->Method_Init		==NULL
		
    ){  
        /* Link the Method */
        Dev_DjiC610620->Method_Init 	= Module_DjiC610620Init;
    }
    else{
        rt_kprintf("Warning: Module Motor 3508 is Configed twice\n");
        return RT_ERROR;
    }

    /* Device Init */
    Dev_DjiC610620->Method_Init(Dev_DjiC610620);
	
	/* Module PID controler Init */
	if(Dev_DjiC610620->Mode == DjiC610620MODE_SPEED)
	{
		Module_PID_Config(&(Dev_DjiC610620->PID_Speed));
	}
	else if(Dev_DjiC610620->Mode == DjiC610620MODE_ANGLE)
	{
		Module_PID_Config(&(Dev_DjiC610620->PID_Speed));
		Module_PID_Config(&(Dev_DjiC610620->PID_Angle));
	}
	
    return RT_EOK;
}

/* Static Method */
static void Module_DjiC610620Init(MODULE_DjiC610620 *module)
{	
	module->Value_motor_AimAngle = 0;
	module->Value_motor_AimCurrent = 0;
	module->Value_motor_AimRPM = 0;
	
	if(module->ENCODER == ROTARYENCODER)
	{	
		module->Encoder_dev = rt_device_find(module->Encoder_DevName);
		if (module->Encoder_dev == RT_NULL){
			rt_kprintf("pulse encoder sample run failed! can't find %s device!\n", module->Encoder_DevName);
			return;
		}
		rt_err_t ret = rt_device_open(module->Encoder_dev, RT_DEVICE_OFLAG_RDONLY);
		if (ret != RT_EOK){
			rt_kprintf("open %s device failed!\n", module->Encoder_DevName);
			return;
		}
		
		rt_pin_mode(GET_PIN(D,12), PIN_MODE_INPUT_PULLUP);
		rt_pin_mode(GET_PIN(D,13), PIN_MODE_INPUT_PULLUP);
	}
}

static void Module_DjiC610620GroupSend(MODULE_DjiC610620GROUP *module)
{
	rt_size_t  size;
	
	/* Calculate the final current value of motor */	
	for(int i=0;i<8;i++)
	{
		if((module->Value_motorUsedMask & (0x01<<i))!=0x00)
		{
			module->Value_module_DjiC610620[i].Value_motor_AimCurrent = 
				module->Value_module_DjiC610620[i].PID_Speed.Value_output;					
		}		
	}
	
	/* make up the all can fram */
	if((module->Value_motorUsedMask & 0x0F) != 0)
	{	
		for(int i=1;i<=4;i++)
		{
			module->Value_can_msgLow.data[i*2-1] = module->Value_module_DjiC610620[i-1].Value_motor_AimCurrent & 0xFF;
			module->Value_can_msgLow.data[i*2-2] = module->Value_module_DjiC610620[i-1].Value_motor_AimCurrent >> 8;
		}
		
		size = rt_device_write(module->Value_can_dev, 0, &(module->Value_can_msgLow), sizeof(module->Value_can_msgLow));
		if (size == 0)
			rt_kprintf("can dev write data failed!\n");
		size = 0;
	}
	
	rt_thread_mdelay(1);
	
	if((module->Value_motorUsedMask & 0xF0) != 0)
	{	
		for(int i=5;i<=8;i++)
		{
			module->Value_can_msgHigh.data[i*2-9] = module->Value_module_DjiC610620[i-1].Value_motor_AimCurrent & 0xFF;
			module->Value_can_msgHigh.data[i*2-10] = module->Value_module_DjiC610620[i-1].Value_motor_AimCurrent >> 8;
		}
					
		size = rt_device_write(module->Value_can_dev, 0, &(module->Value_can_msgHigh), sizeof(module->Value_can_msgHigh));
		if (size == 0)
			rt_kprintf("can dev write data failed!\n");
		size = 0;
	}
}

static rt_err_t Module_DjiC610620GroupHandle(rt_device_t dev, rt_size_t size)
{
	rt_sem_release(&can3508Motor_sem);
		
	return RT_EOK;
}

static void Module_DjiC610620GroupFeed(MODULE_DjiC610620GROUP *module)
{	
	rt_sem_take(&can3508Motor_sem, RT_WAITING_FOREVER);
	
	/* Get true value of the motor */
	rt_uint8_t index = 1;
	rt_device_read(module->Value_can_dev, 0, &(module->Value_can_mrg), sizeof(module->Value_can_mrg));
	index = (module->Value_can_mrg.id & 0x00F) - 1;
	
	module->Value_module_DjiC610620[index].Value_motor_RealAngle = 
		((module->Value_can_mrg.data[0]<<8) | module->Value_can_mrg.data[1]);
	module->Value_module_DjiC610620[index].Value_motor_RealRPM = 
		((module->Value_can_mrg.data[2]<<8) | module->Value_can_mrg.data[3]);
	module->Value_module_DjiC610620[index].Value_motor_RealCurrent = 
		((module->Value_can_mrg.data[4]<<8) | module->Value_can_mrg.data[5]);
	module->Value_module_DjiC610620[index].Value_motor_Temperature = 
		module->Value_can_mrg.data[6];
	
	
	/* Get the offset angle */
	if(module->Value_module_DjiC610620[index].Value_motor_InitFlag == 0)
	{
		module->Value_module_DjiC610620[index].Value_motor_OffsetAngle = 
			module->Value_module_DjiC610620[index].Value_motor_RealAngle;
		module->Value_module_DjiC610620[index].Value_motor_InitFlag = 1;
		return;
	}
	
	if(module->Value_module_DjiC610620[index].Mode == DjiC610620MODE_SPEED)
	{
		/* Update PID output */
		module->Value_module_DjiC610620[index].PID_Speed.Method_Update(
			&(module->Value_module_DjiC610620[index].PID_Speed),
			module->Value_module_DjiC610620[index].Value_motor_AimRPM,
			module->Value_module_DjiC610620[index].Value_motor_RealRPM);	
	}
	else if(module->Value_module_DjiC610620[index].Mode == DjiC610620MODE_ANGLE)
	{
		/* Deal with the data of motor's angle */
		
		if(module->Value_module_DjiC610620[index].ENCODER == DjiC610620SELF)
		{
			/* Use DjiC610/C620 it self encoder */
			module->Value_module_DjiC610620[index].Value_motor_RealAngle -= 
			module->Value_module_DjiC610620[index].Value_motor_OffsetAngle;
		
			if(module->Value_module_DjiC610620[index].Value_motor_RealAngle - 
					module->Value_module_DjiC610620[index].Value_motor_LastAngle > 4096)
						module->Value_module_DjiC610620[index].Value_motor_AngleCnt --;
			else if (module->Value_module_DjiC610620[index].Value_motor_RealAngle - 
					module->Value_module_DjiC610620[index].Value_motor_LastAngle < -4096)
						module->Value_module_DjiC610620[index].Value_motor_AngleCnt ++;
			
			module->Value_module_DjiC610620[index].Value_motor_LastAngle = 
				module->Value_module_DjiC610620[index].Value_motor_RealAngle;
			
			module->Value_module_DjiC610620[index].Value_motor_TotalAngle = 
				(	module->Value_module_DjiC610620[index].Value_motor_AngleCnt * 8192 + 
					module->Value_module_DjiC610620[index].Value_motor_RealAngle)*360/8191;
		}		
		else if(module->Value_module_DjiC610620[index].ENCODER == ROTARYENCODER)
		{
			/* Not use DjiC610/C620 it self encoder but use extra rotary encoder*/
			
			//"module->Value_module_DjiC610620[index].Value_motor_TotalAngle" get from the out side 
			
		}
		
		/* Update PID output */
		module->Value_module_DjiC610620[index].PID_Angle.Method_Update(
			&(module->Value_module_DjiC610620[index].PID_Angle),
			module->Value_module_DjiC610620[index].Value_motor_AimAngle,
			module->Value_module_DjiC610620[index].Value_motor_TotalAngle);	
		
		module->Value_module_DjiC610620[index].Value_motor_AimRPM = 
			module->Value_module_DjiC610620[index].PID_Angle.Value_output;
		
		module->Value_module_DjiC610620[index].PID_Speed.Method_Update(
			&(module->Value_module_DjiC610620[index].PID_Speed),
			module->Value_module_DjiC610620[index].Value_motor_AimRPM,
			module->Value_module_DjiC610620[index].Value_motor_RealRPM);
	}
	
}


/************************ (C) COPYRIGHT 2020 WANGXI **************END OF FILE****/
